#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <unistd.h>
#include <sys/stat.h>

#include <ros/ros.h>
// #include <opencv2/opencv.hpp>
#include <std_msgs/Header.h>
#include <std_msgs/Int8.h>
#include <std_msgs/String.h>
#include <std_msgs/Bool.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>

#include "usb_cam.h"


bool g_exit = false;
void sigint_handler(int sig) {
    printf("捕获到 SIGINT 信号\n");
    printf("处理 SIGINT 信号...\n");
    g_exit = true;
}
bool g_collect_flag_ = false;
std::string g_save_img_;

void callbackRecordStart(const std_msgs::String &msg);
void callbackRecordStop(const std_msgs::String &msg);
bool createFolder(const std::string& folderPath);

int main(int argc, char* argv[])
{
    struct sigaction new_action, old_action;

    // 设置新的信号处理程序
    new_action.sa_handler = sigint_handler;
    sigemptyset(&new_action.sa_mask); // 清空信号屏蔽集
    new_action.sa_flags = 0;

    // 设置SIGINT信号的新处理程序
    if (sigaction(SIGINT, &new_action, &old_action) == -1) {
        perror("无法设置信号处理程序");
        return 1;
    }

    ros::init(argc, argv, "usb_cam_node");
    ros::NodeHandle node("~");
    std::string name;
    int width, height, frame, exposure_auto, exposure_absolute, pub_hz;
    bool set_exposure_flag;
    node.param<std::string>("camera/device_name", name, "/dev/video0");
    node.param<int>("camera/width", width, 1920);
    node.param<int>("camera/height", height, 1200);
    node.param<int>("camera/frame", frame, 5);
    node.param<bool>("camera/set_exposure_flag", set_exposure_flag, false);
    node.param<int>("camera/exposure_auto", exposure_auto, 1);
    node.param<int>("camera/exposure_absolute", exposure_absolute, 60);
    // node.param<std::string>("save_path", g_save_img_, "/home/whur/data/img");
    std::string topic;
    node.param<std::string>("camera/pub_topic", topic, "camera0/raw_image");
    node.param<int>("camera/pub_hz", pub_hz, 10);
    
    ros::Publisher pub_time  = node.advertise<std_msgs::Header>("/camera/time", 10);
    // ros::Publisher pub_status  = node.advertise<std_msgs::Int8>("/camera/status", 10);
    ros::Publisher image_pub = node.advertise<sensor_msgs::CompressedImage>(topic, 1);
    ros::Subscriber sub_start = node.subscribe("/camera/start_record", 1, callbackRecordStart);
    ros::Subscriber sub_stop = node.subscribe("/camera/stop_record", 1, callbackRecordStop);
    std::shared_ptr<usb_cam::UsbCam> app = std::make_shared<usb_cam::UsbCam>(name, width, height, frame);
    app->set_exposure(set_exposure_flag, exposure_auto, exposure_absolute);
    app->configure();
    app->start();
    char* yuyv_buffer = new char[app->get_image_width() * app->get_image_height() *3];
    ros::Rate loop_rate(pub_hz);
    while (ros::ok())
    {   
        if(g_exit)
            break;
        
        app->get_image(yuyv_buffer);
        // printf("len:%ld\n", app->get_image_size_in_bytes());
        sensor_msgs::CompressedImage img_msg;
        img_msg.header.stamp = ros::Time(app->get_image_stamp());
        img_msg.header.frame_id = "camera_optical_frame"; // 通常相机数据的坐标系.
        img_msg.format = "jpeg";
        img_msg.data.resize(app->get_image_size_in_bytes());
        memcpy(&img_msg.data[0], yuyv_buffer, app->get_image_size_in_bytes());
        image_pub.publish(img_msg);
        
        ros::spinOnce();
        loop_rate.sleep();
    }
    delete[] yuyv_buffer;
    return 0;
}

void callbackRecordStart(const std_msgs::String &msg)
{
    if(msg.data != ""){
        printf("start record camera\n");
        g_collect_flag_ = true;
        g_save_img_ = msg.data;
        // createFolder(g_save_img_);
    }
}

void callbackRecordStop(const std_msgs::String &msg)
{
    printf("stop record camera\n");
    g_collect_flag_ = false;
}

bool createFolder(const std::string& folderPath) 
{
    struct stat info;
    if (stat(folderPath.c_str(), &info) != 0) {
        if (mkdir(folderPath.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) == 0) {
            std::cout << "文件夹已创建" << std::endl;
            return true;
        }
        else {
            std::cerr << "文件夹创建失败" << std::endl;
            return false;
        }
    }
    else if (info.st_mode & S_IFDIR) {
        std::cout << "文件夹已存在" << std::endl;
        return true;
    }
    else {
        std::cerr << "路径已存在但不是文件夹" << std::endl;
        return false;
    }
}